
#include <stdio.h>
#include <stdlib.h>

#include <QThread>
#include <QtGui>
#include <QDebug>
#include <QGraphicsSceneMouseEvent>
#include <QPlainTextEdit>
#include <QGraphicsView>
#include <QGraphicsItem>

#include "mainwindow.h"
#include "qgraphicsview.h"

std::vector<NS_APF::Vec2> obstacle_temp = {
    NS_APF::Vec2(100, 100),
    NS_APF::Vec2(200, 200),
    NS_APF::Vec2(300, 300),
    NS_APF::Vec2(500, 400),

};

std::vector<NS_APF::Vec2> init_temp = {
    NS_APF::Vec2(0, 0),
    NS_APF::Vec2(-30, -30),
    NS_APF::Vec2(-30, 30),
    NS_APF::Vec2(-60, -60),
    NS_APF::Vec2(-60, 60)

};

NS_APF::APF_Param apf_param = {
    .obstacle_position_= obstacle_temp,
    .init_UAV_pos_ = init_temp,
    .k_a_ = 200,
    .k_r_ = 1900,
    .k_s_ = 100,
    .UAV_virtual_mass = 5,
    .goal_max_distance_ = 10,
    .detect_radius_ = 60,
    .UAV_max_speed_ = 10,
    .obstacle_dilation_ = 200,
};
float dt = 0.02;
int num = apf_param.init_UAV_pos_.size();
NS_APF::Vec2 goal(700,500);
NS_APF::APF myAPF(&apf_param, goal, num-1);
std::vector<NS_APF::Vec2> position_lists(num, NS_APF::Vec2(0,0));

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

MainWindow::MainWindow(QWidget *parent)
    : QWidget(parent)
{
    // create background image as canvas
    canvasWidth  = 800;
    canvasHeight = 600;
    m_img = new QImage(canvasWidth, canvasHeight,
                       QImage::Format_RGB888);
    m_img->fill(QColor(0xff, 0xff, 0xff));
    setGeometry(20, 35, canvasWidth+20, canvasHeight+20);


    // set window properties
    setBackgroundRole(QPalette::Base);
    setAutoFillBackground(true);
    setFocus();

    // set focus to self
    myAPF.add_obstacle(NS_APF::Vec2(100, 100), NS_APF::Vec2(250 , 200));

    // start timer
    startTimer(2);
}



void MainWindow::timerEvent(QTimerEvent *event)
{
    Q_UNUSED(event);
    static int step_i = 0;

    // run new time step calculation
    std::vector<NS_APF::Vec2> velocity = myAPF.calculate(position_lists);
    for(int i = 0; i < num; i++){
        position_lists[i] = position_lists[i] + velocity[i] * dt;
//        std::cout << "position_lists[" <<i<<"] = "<< position_lists[i]<<std::endl;
    }
    position_box = position_lists;



    // draw
    QPainter    painter;
    m_img->fill(QColor(0xff, 0xff, 0xff));

    painter.begin(m_img);
    painter.setRenderHint(QPainter::Antialiasing, true);

    //draw first plane
    int bis = 6;
    QPolygon triangle;
    triangle << QPoint(position_box[0].x + bis, position_box[0].y)
             << QPoint(position_box[0].x - bis/2, position_box[0].y + bis/2*sqrt(3))
             << QPoint(position_box[0].x - bis/2, position_box[0].y - bis/2*sqrt(3));
    painter.setBrush(Qt::red);
    painter.drawPolygon(triangle);

    // draw follow plane

    for(int i = 1; i < position_box.size(); i++) {
        QPolygon triangle;
        triangle << QPoint(position_box[i].x + bis, position_box[i].y)
                 << QPoint(position_box[i].x - bis/2, position_box[i].y + bis/2*sqrt(3))
                 << QPoint(position_box[i].x - bis/2, position_box[i].y - bis/2*sqrt(3));
        painter.setBrush(Qt::green);
        painter.drawPolygon(triangle);
    }

    //draw connection
    QPen pen;
    pen.setWidth(1);
    pen.setColor(QColor(192,192,192)); //gray
    painter.setPen(pen);
    for(int i = 0; i < myAPF.adjacent_.size(); i++) {
        for(int j = 0; j < myAPF.adjacent_[i].size(); j++){
            std::cout<< "i = "<<i<<"  "<< "j = "<<j<< "  " <<myAPF.adjacent_.at(i).at(j)<< std::endl;
            if(myAPF.adjacent_[i][j] == 1){
                painter.drawLine(position_box[i].x, position_box[i].y, position_box[j].x, position_box[j].y);
            }
        }
    }

    //draw obstacle
    pen.setWidth(4);
    pen.setColor(QColor(0, 0, 0));//black
    painter.setPen(pen);
    for(auto b : myAPF.param_.obstacle_position_) {
        painter.drawPoint(b.x, b.y);
    }

    //draw goal point
    pen.setWidth(10);
    pen.setColor(QColor(0, 0, 255));//blue
    painter.setPen(pen);
    painter.drawPoint(myAPF.goal_.x,myAPF.goal_.y);

    painter.end();

    // save images
    if( 0 ) {
        char img_name[256];
        sprintf(img_name, "img_%06d.jpg", step_i++);
        m_img->save(QString::fromLocal8Bit(img_name));
    }

    // update to mainwindow
    this->update();
}




void MainWindow::keyPressEvent(QKeyEvent *event)
{
    QPainter painter;

    // 'o' - random draw rectangles
    if( event->key() == Qt::Key_O ) {
        painter.begin(m_img);
        painter.setRenderHint(QPainter::Antialiasing, true);

        double  x1, y1, x2, y2, w, h;

        for(int i=0; i<1000; i++) {
            painter.setPen(QColor(rand()%255, rand()%255, rand()%255, rand()%255));

            x1 = rand() % 800; y1 = rand() % 600;
            x2 = rand() % 800; y2 = rand() % 600;
            w = x2 - x1;
            h = y2 - y1;
            painter.drawRect(x1, y1, w, h);
        }

        painter.end();
    }

    // 'c' - clean canvas
    if( event->key() == Qt::Key_C ) {
        m_img->fill(QColor(0xff, 0xff, 0xff));
    }

    // 'q' - quit the program
    if( event->key() == Qt::Key_Q ) {
        this->close();
    }



    this->update();
}

void MainWindow::mousePressEvent(QMouseEvent *event)
{
    myAPF.set_goal(NS_APF::Vec2(event->x(), event->y()));
}

void MainWindow::mouseMoveEvent(QMouseEvent *event)
{
    Q_UNUSED(event);
}

void MainWindow::paintEvent(QPaintEvent *event)
{
    Q_UNUSED(event);

    QPainter    painter(this);

    // draw offline image to screen
    painter.drawImage(QPoint(0, 0), *m_img);
}

